#include "Physics.h"
#include "gameworld.h"
#include "Octree.h"
#include "AABBCollider.h"
#include "landscape.h"
#include "terrain.h"
#include "robot.h"
#include "Key.h"
float lastPercent = 0.0f;
float currentPercent = 0.0f;

Physics::Physics(void)
{
	collisions = new stack<Collision*>();
	entitiesToRemove = new vector<Entity*>();
	for(int i=0;i<300;i++){
		collisions->push(new Collision());
		frameCollisions[i] = NULL;
	}
}

Physics::~Physics()
{
	int i;
	for(i=0;i<300;i++){
		Collision* c = collisions->top();
		delete c;
		collisions->pop();
	}
	delete entitiesToRemove;
	delete collisions;
}

float Physics::sqDistPointToBox(float sx,float sy, float sz, float cx, float cy, float cz, float w, float h, float d)
{
	float dist = 0.0f;

	//w /= 2.0f;
	//h /= 2.0f;
	//d /= 2.0f;

	float minx = cx - w;
	float miny = cy - h;
	float minz = cz - d;
	float maxx = cx + w;
	float maxy = cy + h;
	float maxz = cz + d;

	if(sx < minx){
		dist += (minx - sx) * (minx - sx);
	}
	if(sx > maxx){
		dist += (sx - maxx) * (sx - maxx);
	}

	if(sy < miny){
		dist += (miny - sy) * (miny - sy);
	}
	if(sy > maxy){
		dist += (sy - maxy) * (sy - maxy);
	}

	if(sz < minz){
		dist += (minz - sz) * (minz - sz);
	}
	if(sz > maxz){
		dist += (sz - maxz) * (sz - maxz);
	}

	return dist;
}


int Physics::removeCollisionsWithEntity(int numCols, Entity* entity)
{
	if(entity == NULL ){
		return numCols;
	}

	int i=0;
	for(i=0;i<100;i++){
		Collision* c = frameCollisions[i];
		if(c == NULL) break;
		if(c->ent1 == entity || c->ent2 == entity){
			frameCollisions[i] = NULL;
			collisions->push(c);
			numCols--;
		}
	}
	
	int currentNull = 0;
	for(i=1;i<100;i++){
		Collision* c = frameCollisions[i];
		if(c != NULL){
			int j;
			for(j=currentNull;j<i;j++){
				if(frameCollisions[j] == NULL){
					frameCollisions[j] = c;
					frameCollisions[i] = NULL;
					currentNull = j;
					break;
				}
			}
		}
	}
	return numCols;
}

void Physics::sort(int num)
{
	  int i, j;
	  int flag = 1;
      Collision* temp;
      
      for(i = 1; (i <= num) && flag; i++){
          flag = 0;
          for (j=0; j < (num -1); j++){
			 if (frameCollisions[j+1]->start < frameCollisions[j]->start){ 
                    temp = frameCollisions[j];
                    frameCollisions[j] = frameCollisions[j+1];
                    frameCollisions[j+1] = temp;
                    flag = 1;
               }
          }
     }

}

void Physics::update(GameWorld* game)
{
	Octree* octree = game->getOctree();
	
	list<Entity*>* entities = octree->getEntities();

	int num = entities->size();
	
	Entity* entity;

	list<Entity*>::iterator it;
	for (it=entities->begin() ; it != entities->end(); it++ ){
		entity = *it;
		
		//if(sprite->getPhysicalProperties()->getVelocityX() != 0){
		//	sprite->getPhysicalProperties()->setGround(false);
		//}
		if(entity->getGravity()){
			Vector3 vel = entity->getVelocity();
			vel.y -= GRAVITY;
			entity->setVelocity(vel.x,vel.y,vel.z);
		}
	}

	lastPercent = 0.0f;
	currentPercent = 0.0f;

	int numCols = getEntityCollisions(game,num,0);
	
	sort(numCols);

	while(numCols>0 && currentPercent <= 1.0f){
		
		Entity* ent1 = frameCollisions[0]->ent1;
		Entity* ent2 = frameCollisions[0]->ent2;

		moveEntitiesByPercent(game, currentPercent - lastPercent);
		fixCollision(game,frameCollisions[0]);

		numCols = removeCollisionsWithEntity(numCols,ent1);
		numCols = removeCollisionsWithEntity(numCols,ent2);

		int before = numCols;
		numCols = getNewEntityCollisions(game, numCols, ent2, currentPercent);
		numCols = getNewEntityCollisions(game, numCols, ent2, currentPercent);

		if(numCols > before){
			sort(numCols);
		}
		
		lastPercent = currentPercent;
				
	}

	if(currentPercent < 1.0f){
		moveEntitiesByPercent(game,1.0f-currentPercent);
	}

	//list<Entity*>* entities = octree->getEntities();

	//Entity* entity;

	//list<Entity*>::iterator it;
	Terrain* terrain = game->getLandscape()->getTerrain();
	for (it=entities->begin() ; it != entities->end(); it++ ){
		entity = *it;

		if(entity->getType() == PLATFORM) continue;

					Vector3 entityPos = entity->getPosition();
					float terrainHeight = terrain->getHeightAt(entityPos.x, entityPos.z);
					float entityRadius;

					if(entity->getCollider()->m_type == SPHERE){
		
						entityRadius = entity->getCollider()->getRadius();

		
					}
					else if(entity->getCollider()->m_type == AABB){
						AABBCollider* c = (AABBCollider*)entity->getCollider();
						entityRadius = c->getHeight() / 2.0f;
					}	
			if(terrainHeight > entityPos.y - entityRadius){
				entityPos.y = terrainHeight + entityRadius;
				entity->setPosition(entityPos);
				entity->setVelocity(entity->getVelocity().x,0.0f,entity->getVelocity().z);
				
				switch(entity->getType()){
				case ROCKET:
					entity->collide(NULL);
					entitiesToRemove->push_back(entity);
					break;
				default:
					entity->setJumping(false);
					break;
				}
			}
	}
	for(int i=0; i<entitiesToRemove->size(); i++){
		octree->removeCollider(entity->getCollider());
		octree->removeEntity(entity);
	}
	entitiesToRemove->clear();
}

void Physics::moveEntitiesByPercent(GameWorld* game,float percent)
{
	Octree* octree = game->getOctree();
	
	list<Entity*>* entities = octree->getEntities();

	Entity* entity;

	list<Entity*>::iterator it;
	for (it=entities->begin() ; it != entities->end(); it++ ){
		entity = *it;
		if(entity->getType() == PLAYER){
			int s =2;
		}
		Vector3 vel = entity->getVelocity();
		Vector3 pos = entity->getPosition();
		
		pos.x += vel.x * percent;
		pos.y += vel.y * percent;
		pos.z += vel.z * percent;

		entity->setPosition(pos);
	}
}

int Physics::getEntityCollisions(GameWorld* game, int numSprites, int numCols)
{
	Octree* octree = game->getOctree();

	list<Entity*>* entities = octree->getEntities();

	Entity* entity1;
	Entity* entity2;

	list<Collider*>* colliders = octree->getColliders();
	list<Collider*>::iterator it;
	for (it=colliders->begin() ; it != colliders->end(); it++ ){
		entity1 = (*it)->getEntity();
		if(entity1->getType()==ROCKET){
			int s=2;
		}
		AABBCollider* col1 = (AABBCollider*)entity1->getCollider();

		list<Collider*>* colliders = octree->getColliders();
		list<Collider*>::iterator it2;
	for (it2=colliders->begin() ; it2 != colliders->end(); it2++ ){
		entity2 = (*it2)->getEntity();
			
			if(entity1 == entity2){
				continue;
			}

			
			AABBCollider* col2 = (AABBCollider*)entity2->getCollider();

			Vector3 pos1 = entity1->getPosition();
			Vector3 pos2 = entity2->getPosition();

			float x1 = pos1.x - (col1->getWidth() / 2.0f);
			float y1 = pos1.y - (col1->getHeight() / 2.0f);
			float z1 = pos1.z - (col1->getDepth() / 2.0f);

			float x2 = pos2.x - (col2->getWidth() / 2.0f);
			float y2 = pos2.y - (col2->getHeight() / 2.0f);
			float z2 = pos2.z - (col2->getDepth() / 2.0f);

			Vector3 vel1 = entity1->getVelocity();
			Vector3 vel2 = entity2->getVelocity();

			float velX = vel1.x - vel2.x;
			float velY = vel1.y - vel2.y;
			float velZ = vel1.z - vel2.z;

			

			float maxx1 = pos1.x + col1->getWidth() / 2.0f;
			float maxx2 = pos2.x + col2->getWidth() / 2.0f;
			float maxy1 = pos1.y + col1->getHeight() / 2.0f;
			float maxy2 = pos2.y + col2->getHeight() / 2.0f;
			float maxz1 = pos1.z + col1->getDepth() / 2.0f;
			float maxz2 = pos2.z + col2->getDepth() / 2.0f;

			float startx = 0.0f,starty= 0.0f,startz = 0.0f, endx= 0.0f,endy= 0.0f,endz = 0.0f,time= 0.0f;

			if(maxx1 < x2){
					if(velX > 0){
						float dx = (float)x2 - maxx1;
						startx = dx / velX;
	
						dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						startx = NAN;
						endx = NAN;
					}
				}
				else if(x1 > maxx2){
					if(velX < 0){
						float dx = (float)(x1 - maxx2);
						startx = -(dx / velX);
	
						dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else{
						startx = NAN;
						endx = NAN;
					}
				}
				else if(x1 > x2 && maxx1 < maxx2){
					startx = 0.0f;
					if(velX < 0){
						float dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else if(velX > 0){
						float dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						endx = FLT_MAX;
					}
				}
				else if(x1 < x2 && maxx1 > maxx2){
					startx = 0.0f;
					if(velX < 0){
						float dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else if(velX > 0){
						float dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						endx = FLT_MAX;
					}
				}
				else if(maxx1 >= x2 && maxx1 <= maxx2){
					startx = 0.0f;
	
					if(velX < 0){
						float dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else if(velX > 0){
						float dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						endx = FLT_MAX;
					}
				}
				else if(x1 <= maxx2 && x1 >= x2){
					startx = 0.0f;
	
					if(velX < 0){
						float dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else if(velX > 0){
						float dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						endx = FLT_MAX;
					}
				}
	
	
	
	
				if(maxy1 < y2){
					if(velY > 0){
						float dy = (float)y2 - maxy1;
						starty = dy / velY;
	
						dy = (float)(maxy2 - y1);
						endy = dy / velY;
					}
					else{
						starty = NAN;
						endy = NAN;
					}
				}
				else if(y1 > maxy2){
					if(velY < 0){
						float dy = (float)(y1 - maxy2);
						starty = -(dy / velY);
	
						dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else{
						starty = NAN;
						endy = NAN;
					}
				}
				else if(y1 > y2 && maxy1 < maxy2){
					starty = 0.0f;
					if(velY < 0){
						float dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else if(velY > 0){
						float dy = (float)(maxy2 - y1);
						endy = dy / velY;
					}
					else{
						endy = FLT_MAX;
					}
				}
				else if(y1 < y2 && maxy1 > maxy2){
					starty = 0.0f;
	
					if(velY < 0){
						float dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else if(velY > 0){
						float dy = (float)(maxy2 - y1);
						endy = dy / velY;
					}
					else{
						endy = FLT_MAX;
					}
				}
				else if(maxy1 >= y2 && maxy1 <= maxy2){
					starty = 0.0f;
	
					if(velY < 0){
						float dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else if(velY > 0){
						float dy = (float)(maxy2 - y1);
						endy = -(dy / velY);
					}
					else{
						endy = FLT_MAX;
					}
				}
				else if(y1 <= maxy2 && y1 >= y2){
					starty = 0.0f;
	
					if(velY < 0){
						float dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else if(velY > 0){
						float dy = (float)(maxy2 - y1);
						endy = dy / velY;
					}
					else{
						endy = FLT_MAX;
					}
				}
	
	
	
	
				if(maxz1 < z2){
					if(velZ > 0){
						float dz = (float)z2 - maxz1;
						startz = dz / velZ;
	
						dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						startz = NAN;
						endz = NAN;
					}
				}
				else if(z1 > maxz2){
					if(velZ < 0){
						float dz = (float)(z1 - maxz2);
						startz = -(dz / velZ);
	
						dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else{
						startz = NAN;
						endz = NAN;
					}
				}
				else if(z1 > z2 && maxz1 < maxz2){
					startz = 0.0f;
					if(velZ < 0){
						float dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else if(velZ > 0){
						float dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						endz = FLT_MAX;
					}
				}
				else if(z1 < z2 && maxz1 > maxz2){
					startz = 0.0f;
	
					if(velZ < 0){
						float dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else if(velZ > 0){
						float dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						endz = FLT_MAX;
					}
				}
				else if(maxz1 >= z2 && maxz1 <= maxz2){
					startz = 0.0f;
	
					if(velZ < 0){
						float dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else if(velZ > 0){
						float dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						endz = FLT_MAX;
					}
				}
				else if(z1 <= maxz2 && z1 >= z2){
					startz = 0.0f;
	
					if(velZ < 0){
						float dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else if(velZ > 0){
						float dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						endz = FLT_MAX;
					}
				}
	
				if(startx <= 1.0f && starty <= 1.0f && startz <= 1.0f){
					if(startx >= starty && startx >= startz){
						if(startx <= endy && startx <= endz){
							time = startx;
						}
						else{
							time = -1.0f;
						}
					}
					else if(starty >= startz){
						if(starty <= endx && starty <= endz){
							time = starty;
						}
						else{
							time = -1.0f;
						}
					}
					else{
						if(startz <= endx && startz <= endy){
							time = startz;
						}
						else{
							time = -1.0f;
						}
					}
				}
				else{
					time = -1.0f;
				}

			if(time > 0.0f && time <= 1.0f){
				Collision* c = collisions->top();
				collisions->pop();
							
				c->ent1 = entity1;
				c->ent2 = entity2;
				c->start = time;

				frameCollisions[numCols] = c;
				numCols++;
			}

		}
	}

	return numCols;
}


int Physics::getNewEntityCollisions(GameWorld* game, int numCols, Entity* entity, float currentPercent)
{
	if(entity == NULL){
			return numCols;
	}

	Octree* octree = game->getOctree();
	
	list<Entity*>* entities = octree->getEntities();

	Entity* entity2;

	AABBCollider* c1 = (AABBCollider*)entity->getCollider();

	float x1 = entity->getPosition().x - (c1->getWidth() / 2.0f);
	float y1 = entity->getPosition().y - (c1->getHeight() / 2.0f);
	float z1 = entity->getPosition().z - (c1->getDepth() / 2.0f);
		
	float velX1 = entity->getVelocity().x * (1.0f - currentPercent);
	float velY1 = entity->getVelocity().y * (1.0f - currentPercent);
	float velZ1 = entity->getVelocity().z * (1.0f - currentPercent);

	list<Collider*>* colliders = octree->getColliders();
	list<Collider*>::iterator it;
	for (it=colliders->begin() ; it != colliders->end(); it++ ){
		entity2 = (*it)->getEntity();
		
				if(entity2 == entity){
					continue;
				}
		
				
				AABBCollider* c2 = (AABBCollider*)entity2->getCollider();

				float x2 = entity2->getPosition().x  - (c2->getWidth() / 2.0f);
				float y2 = entity2->getPosition().y - (c2->getHeight() / 2.0f);
				float z2 = entity2->getPosition().z - (c2->getDepth() / 2.0f);
				
				float velX2 = entity2->getVelocity().x * (1.0f - currentPercent);
				float velY2 = entity2->getVelocity().y * (1.0f - currentPercent);
				float velZ2 = entity2->getVelocity().z * (1.0f - currentPercent);
				
				float velX = velX1 - velX2;
				float velY = velY1 - velY2;
				float velZ = velZ1 - velZ2;
				
				float startx = 0.0f;
				float starty = 0.0f;
				float startz = 0.0f;
				float endx = 0.0f;
				float endy = 0.0f;
				float endz = 0.0f;
				float time = 0.0f;
	
				

				float maxx1 = (float)(x1 + c1->getWidth() / 2.0f);
				float maxy1 = (float)(y1 + c1->getHeight() / 2.0f);
				float maxz1 = (float)(z1 + c1->getDepth() / 2.0f);
				
				float maxx2 = (float)(x2 + c2->getWidth() / 2.0f);
				float maxy2 = (float)(y2 + c2->getHeight() / 2.0f);
				float maxz2 = (float)(z2 + c2->getDepth() / 2.0f);
	
				if(maxx1 < x2){
					if(velX > 0){
						float dx = (float)x2 - maxx1;
						startx = dx / velX;
	
						dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						startx = NAN;
						endx = NAN;
					}
				}
				else if(x1 > maxx2){
					if(velX < 0){
						float dx = (float)(x1 - maxx2);
						startx = -(dx / velX);
	
						dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else{
						startx = NAN;
						endx = NAN;
					}
				}
				else if(x1 > x2 && maxx1 < maxx2){
					startx = 0.0f;
					if(velX < 0){
						float dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else if(velX > 0){
						float dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						endx = FLT_MAX;
					}
				}
				else if(x1 < x2 && maxx1 > maxx2){
					startx = 0.0f;
					if(velX < 0){
						float dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else if(velX > 0){
						float dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						endx = FLT_MAX;
					}
				}
				else if(maxx1 >= x2 && maxx1 <= maxx2){
					startx = 0.0f;
	
					if(velX < 0){
						float dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else if(velX > 0){
						float dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						endx = FLT_MAX;
					}
				}
				else if(x1 <= maxx2 && x1 >= x2){
					startx = 0.0f;
	
					if(velX < 0){
						float dx = (float)(maxx1 - x2);
						endx = -(dx / velX);
					}
					else if(velX > 0){
						float dx = (float)(maxx2 - x1);
						endx = dx / velX;
					}
					else{
						endx = FLT_MAX;
					}
				}
	
	
	
	
				if(maxy1 < y2){
					if(velY > 0){
						float dy = (float)y2 - maxy1;
						starty = dy / velY;
	
						dy = (float)(maxy2 - y1);
						endy = dy / velY;
					}
					else{
						starty = NAN;
						endy = NAN;
					}
				}
				else if(y1 > maxy2){
					if(velY < 0){
						float dy = (float)(y1 - maxy2);
						starty = -(dy / velY);
	
						dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else{
						starty = NAN;
						endy = NAN;
					}
				}
				else if(y1 > y2 && maxy1 < maxy2){
					starty = 0.0f;
					if(velY < 0){
						float dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else if(velY > 0){
						float dy = (float)(maxy2 - y1);
						endy = dy / velY;
					}
					else{
						endy = FLT_MAX;
					}
				}
				else if(y1 < y2 && maxy1 > maxy2){
					starty = 0.0f;
	
					if(velY < 0){
						float dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else if(velY > 0){
						float dy = (float)(maxy2 - y1);
						endy = dy / velY;
					}
					else{
						endy = FLT_MAX;
					}
				}
				else if(maxy1 >= y2 && maxy1 <= maxy2){
					starty = 0.0f;
	
					if(velY < 0){
						float dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else if(velY > 0){
						float dy = (float)(maxy2 - y1);
						endy = -(dy / velY);
					}
					else{
						endy = FLT_MAX;
					}
				}
				else if(y1 <= maxy2 && y1 >= y2){
					starty = 0.0f;
	
					if(velY < 0){
						float dy = (float)(maxy1 - y2);
						endy = -(dy / velY);
					}
					else if(velY > 0){
						float dy = (float)(maxy2 - y1);
						endy = dy / velY;
					}
					else{
						endy = FLT_MAX;
					}
				}
	
	
	
	
				if(maxz1 < z2){
					if(velZ > 0){
						float dz = (float)z2 - maxz1;
						startz = dz / velZ;
	
						dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						startz = NAN;
						endz = NAN;
					}
				}
				else if(z1 > maxz2){
					if(velZ < 0){
						float dz = (float)(z1 - maxz2);
						startz = -(dz / velZ);
	
						dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else{
						startz = NAN;
						endz = NAN;
					}
				}
				else if(z1 > z2 && maxz1 < maxz2){
					startz = 0.0f;
					if(velZ < 0){
						float dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else if(velZ > 0){
						float dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						endz = FLT_MAX;
					}
				}
				else if(z1 < z2 && maxz1 > maxz2){
					startz = 0.0f;
	
					if(velZ < 0){
						float dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else if(velZ > 0){
						float dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						endz = FLT_MAX;
					}
				}
				else if(maxz1 >= z2 && maxz1 <= maxz2){
					startz = 0.0f;
	
					if(velZ < 0){
						float dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else if(velZ > 0){
						float dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						endz = FLT_MAX;
					}
				}
				else if(z1 <= maxz2 && z1 >= z2){
					startz = 0.0f;
	
					if(velZ < 0){
						float dz = (float)(maxz1 - z2);
						endz = -(dz / velZ);
					}
					else if(velZ > 0){
						float dz = (float)(maxz2 - z1);
						endz = dz / velZ;
					}
					else{
						endz = FLT_MAX;
					}
				}
	
				if(startx <= 1.0f && starty <= 1.0f && startz <= 1.0f){
					if(startx >= starty && startx >= startz){
						if(startx <= endy && startx <= endz){
							time = startx + currentPercent;
						}
						else{
							time = -1.0f;
						}
					}
					else if(starty >= startz){
						if(starty <= endx && starty <= endz){
							time = starty + currentPercent;
						}
						else{
							time = -1.0f;
						}
					}
					else{
						if(startz <= endx && startz <= endy){
							time = startz + currentPercent;
						}
						else{
							time = -1.0f;
						}
					}
				}
				else{
					time = -1.0f;
				}
	
		if(time >= 0.0f && time <= 1.0f){
			Collision* c = collisions->top();
			collisions->pop();
							
			c->ent1 = entity;
			c->ent2 = entity2;
			c->start = time;

			frameCollisions[numCols] = c;
			numCols++;
		}
			
	}
	return numCols;
}


void Physics::fixCollision(GameWorld* game, Collision* coll)
{
	short direction = getEntityCollisionDirection(coll);

	if(coll->ent1->getType() == PLAYER || coll->ent2->getType() == PLAYER){
		
		
				if(coll->ent1->getType() != PLAYER){
					swapEntities(coll);
					direction = swapDirections(direction);
				}
				Vector3 pos1 = coll->ent1->getPosition();
				Vector3 pos2 = coll->ent2->getPosition();
				Vector3 vel1 = coll->ent1->getVelocity();
				Vector3 vel2 = coll->ent2->getVelocity();
				if(coll->ent2->getType() == PLATFORM){
					AABBCollider* c = (AABBCollider*) coll->ent2->getCollider();
					switch(direction){
						case BOTTOM:
							coll->ent1->setVelocity(vel1.x,0.0f,vel1.z);
							
							coll->ent1->setPosition(Vector3(pos1.x,pos1.y + 0.001f, pos1.z));
							coll->ent1->setJumping(false);
							break;
						case TOP:
							coll->ent1->setVelocity(vel1.x,0.0f,vel1.z);
							coll->ent1->setPosition(Vector3(coll->ent1->getPosition().x, coll->ent1->getPosition().y - 0.1f, coll->ent1->getPosition().z));
							break;
						case LEFT:
							coll->ent1->setVelocity(0.0f,vel1.y,vel1.z);
							coll->ent1->setPosition(Vector3(pos1.x + 0.1f, pos1.y, pos1.z));
							
							break;
						case RIGHT:
							coll->ent1->setVelocity(0.0f,vel1.y,vel1.z);
							coll->ent1->setPosition(Vector3(pos1.x - 0.1f, pos1.y, pos1.z));
							
							break;
						case FRONT:
							coll->ent1->setVelocity(vel1.x,vel1.y,0.0f);
							coll->ent1->setPosition(Vector3(pos1.x, pos1.y, pos1.z + 0.1f));
							
							break;
						case BACK:
							coll->ent1->setVelocity(vel1.x,vel1.y,0.0f);
							coll->ent1->setPosition(Vector3(pos1.x, pos1.y, pos1.z - 0.1f));
							
							break;
					}
				}
				else if(coll->ent2->getType() == ROCKET){
					coll->ent2->collide(coll->ent1);
					game->getOctree()->removeCollider(coll->ent2->getCollider());
					game->getOctree()->removeEntity(coll->ent2);
					//coll->ent2 = NULL;
				}
				else if(coll->ent2->getType() == KEY){
					coll->ent1->collide(coll->ent2);
					coll->ent2->collide(coll->ent1);
					game->getOctree()->removeCollider(coll->ent2->getCollider());
					game->getOctree()->removeEntity(coll->ent2);
					//coll->ent2 = NULL;
				}
				else if(coll->ent2->getType() == ROBOT){
					game->getOctree()->removeCollider(coll->ent2->getCollider());
					game->getOctree()->removeEntity(coll->ent2);
					game->setGameover(true);
				}
			}
	else if(coll->ent1->getType() == ROCKET || coll->ent2->getType() == ROCKET){
		if(coll->ent1->getType() != ROCKET){
			swapEntities(coll);
			direction = swapDirections(direction);
		}
		if(coll->ent2->getType() == PLATFORM){
			coll->ent1->collide(coll->ent2);
			game->getOctree()->removeCollider(coll->ent1->getCollider());
			game->getOctree()->removeEntity(coll->ent1);
			//coll->ent1 = NULL;
		}
		else if(coll->ent2->getType() == ROBOT){
			coll->ent1->collide(coll->ent2);
			coll->ent2->collide(coll->ent1);

			game->getOctree()->removeCollider(coll->ent2->getCollider());
			game->getOctree()->removeEntity(coll->ent2);
			//coll->ent2 = NULL;
			
			game->getOctree()->removeCollider(coll->ent1->getCollider());
			game->getOctree()->removeEntity(coll->ent1);
			//coll->ent1 = NULL;

			Robot* robot = (Robot*)coll->ent2;
			if(robot->getKey() != -1){
				Key* k = new Key(game,robot->getKey());
				k->initialize();
				k->setPosition(robot->getPosition());
				Octree* octree = game->getOctree();
				octree->addCollider(k->getCollider());
				octree->addEntity(k);
			}
		}
	}
}

short Physics::getEntityCollisionDirection(Collision* coll){
	if(coll->ent1 == NULL || coll->ent2 == NULL) return FRONT;
	Vector3 pos1 = coll->ent1->getPosition();
	Vector3 pos2 = coll->ent2->getPosition();

	AABBCollider* c1 = (AABBCollider*)coll->ent1->getCollider();
	AABBCollider* c2 = (AABBCollider*)coll->ent2->getCollider();
	
	float minx1 = pos1.x - c1->getWidth() / 2.0f;
	float miny1 = pos1.y - c1->getHeight() / 2.0f;
	float minz1 = pos1.z - c1->getDepth() / 2.0f;

	float minx2 = pos2.x - c2->getWidth() / 2.0f;
	float miny2 = pos2.y - c2->getHeight() / 2.0f;
	float minz2 = pos2.z - c2->getDepth() / 2.0f;
	
	float maxx1 = pos1.x + c1->getWidth() / 2.0f;
	float maxy1 = pos1.y + c1->getHeight() / 2.0f;
	float maxz1 = pos1.z + c1->getDepth() / 2.0f;
		
	float maxx2 = pos2.x + c2->getWidth() / 2.0f;
	float maxy2 = pos2.y + c2->getHeight() / 2.0f;
	float maxz2 = pos2.z + c2->getDepth() / 2.0f;
		
		float left = abs((float)(minx1 - maxx2));
		float right = abs((float)(minx2 - maxx1));
		float bottom = abs((float)(miny1 - maxy2));
		float top = abs((float)(miny2 - maxy1));
		float front = abs((float)(minz1 - maxz2));
		float back = abs((float)(minz2 - maxz1));
		
		if(left <= right && left <= bottom && left <= top && left <= front && left <= back){
			return LEFT;
		}
		else if(right <= bottom && right <= top && right <= front && right <= back){
			return RIGHT;
		}
		else if(bottom <= top && bottom <= front && bottom <= back){
			return BOTTOM;
		}
		else if(top <= front && top <= back){
			return TOP;
		}
		else if(front <= back){
			return FRONT;
		}
		else{
			return BACK;
		}
	}
	
	void Physics::swapEntities(Collision* coll)
	{
		Entity* temp = coll->ent1;
		coll->ent1 = coll->ent2;
		coll->ent2 = temp;
	}
	
	short Physics::swapDirections(short dir)
	{
		if(dir == LEFT){
			return RIGHT;
		}
		else if(dir == RIGHT){
			return LEFT;
		}
		else if(dir == TOP){
			return BOTTOM;
		}
		else if(dir == BOTTOM){
			return TOP;
		}
		else if(dir == FRONT){
			return BACK;
		}
		else{
			return FRONT;
		}
	}

	bool Physics::isSorted(Collision** cols){
	int i;
	for(i=0;i<99;i++){
		if(cols[i+1] == NULL) return true;
		if(cols[i]->start > cols[i+1]->start) return false;
	}
	return true;
}